#include"tic_crow/tic_crow.h"
#include <dirent.h>
#include <sys/inotify.h>

#define BUF_LEN 1000

//将robot pose 转化成 像素点pose
Point2f
tic_crow::AppCrow::TransPose(poseinfo p, float t_x, float t_y){

    float x = t_x;
    float y = t_y;
    float h = p.height;
    float w = p.weight;
    float ox = p.originX;
    float oy = p.originY;

    float offset_x = x - ox;
    float offset_y = y - oy;

    float _x = offset_x * 20.00;
    float _y = h -  offset_y *20.00;

    Point2f temp(_x, _y);

    return temp;
}

int 
tic_crow::AppCrow::Denoising(string path, poseinfo p){

    //int _x, _y;
    //将原始数据存到vector中
    Point2f t(p.poseX, p.poseY);
    points.push_back(t);

    vector<Point2f> tempPoints;
    //从像素点转到坐标点
    for(vector<Point2f>::iterator iter = points.begin(); iter != points.end(); iter++){

        Point2f tempPoint = *iter;
        Point2f catPoint = TransPose(p, tempPoint.x, tempPoint.y);
        tempPoints.push_back(catPoint);

    }
    Mat input = imread(path, 0);

    if(tempPoints.size() > 1){

        for(vector<Point2f>::iterator iter = tempPoints.begin(); iter != tempPoints.end()-1; iter++){

            Point beginpoint = *iter;
            Point endpoint = *(iter+1);

            line(input, beginpoint, endpoint, Scalar(255), 10, 4);
        }
    }

    imwrite(path, input);
    return 1;
}

int judgeFile(char const* path){
    struct stat s_buf;
    stat(path,&s_buf);
    if(S_ISDIR(s_buf.st_mode)){
        return 0;//目录
    }else{
        return 1;//普通文件
    }
}

vector<string> 
tic_crow::AppCrow::getFileName(string Path){
	struct dirent *ptr;
	DIR *dir;
	dir=opendir(Path.c_str());
	vector<string> files;
	while((ptr=readdir(dir))!=NULL){
        string map_path = Path +'/'+ptr->d_name;
        int flag = judgeFile(map_path.c_str());
		if(ptr->d_name[0] == '.'||flag)
			continue;
		files.push_back(ptr->d_name);
	}
	closedir(dir);
    sort(files.begin(),files.end());
	return files;
}

vector<string> 
tic_crow::AppCrow::getPathName(string Path){
	struct dirent *ptr;
	DIR *dir;
	dir=opendir(Path.c_str());
	vector<string> files;
    string p_name;
	while((ptr=readdir(dir))!=NULL){
        string map_path = Path +'/'+ptr->d_name;
        int flag = judgeFile(map_path.c_str());
		if(ptr->d_name[0] == '.'||!flag)
			continue;
        p_name = ptr->d_name;
		files.push_back(p_name.substr(0,p_name.rfind(".")));
	}
	closedir(dir);
    sort(files.begin(),files.end());
    return files;
}


bool
tic_crow::AppCrow::monitor_file(const char* path){
    int fd = inotify_init();
    if(fd == -1)
	{
        ROS_INFO("初始化失败");
        return false;
	}
    int wd = inotify_add_watch(fd,path,IN_MODIFY|IN_CLOSE);
	if(wd == -1)
	{
        ROS_INFO("wd读取失败");
        return false;
	}
    char buf[BUF_LEN];
	ssize_t numRead;
	char *p;
	struct inotify_event *event;
	int flags;
    while(1)
	{
		numRead = read(fd,buf,BUF_LEN);
		if(numRead == -1)
		{
			ROS_INFO("读取时间失败");
			break;
		}
 
		ROS_INFO("Read %ldbytes from inotify fd\n",(long)numRead);
		for(p=buf;p < buf+numRead;)
		{
			event = (struct inotify_event *)p;
			if(event->mask & IN_MODIFY)  {
                ROS_INFO("IN_MODIFY\n");
            }
            if(event->mask & IN_CLOSE){
                close(fd);
                ROS_INFO("IN_CLOSE\n");
                return true;//当文件停止修改时退出
            }
            if(event->mask & IN_ACCESS)  {//被访问
                ROS_INFO("IN_ACCESS\n");
            }
            if(event->mask & IN_ATTRIB)  {
                ROS_INFO("IN_ATTRIB\n");
            }
            if(event->mask & IN_OPEN)  {
                ROS_INFO("IN_OPEN\n");
            }
			p+=sizeof(struct inotify_event) + event->len;
		}
	}
	close(fd);
}

bool 
tic_crow::AppCrow::ProcessIsRun(const char *proc)
{
    FILE *fp = NULL;
    unsigned int count;
    char buf[BUFSZ];
    char command[150];
    
    sprintf(command, "ps -ef |grep -w %s |grep -v  grep |wc -l",proc);

    if((fp = popen(command,"r")) == NULL) {
        return false;
        }
    if((fgets(buf,BUFSZ,fp)) != NULL) {
        // std::cout<<"command fp:" <<fp << std::endl;
        count = atoi(buf);
        // std::cout<<"buf count:" <<count << std::endl;
    }  
    pclose(fp);
    fp = NULL;
    if(count == 0) {
        return true;  //不存在进程是true
    } else {
        return false;
    }
}


bool 
tic_crow::AppCrow::RenameFile(const char* old_name,const char* new_name){
	if( access(old_name,F_OK)==0){

		int flag = rename(old_name, new_name);
		cout << "status of rename: " << flag <<endl;
		if(!flag){
			cout << "rename successfully" << endl;
			return true;
		}
		else{	
			cout << "fail to rename" << endl;
			return false;
		}
	}
	else{
		cout << "The file you expected does not exist!!!" << endl;
		return false;
	}
}

//LINUX下历遍目录 opendir -> readdir -> closedir 
// 打开 ->  读取  -> 关闭
 
void 
tic_crow::AppCrow::Getfilepath(const char *path, const char *filename,  char *filepath)
{
    strcpy(filepath, path);
    if(filepath[strlen(path) - 1] != '/')
        strcat(filepath, "/");
    strcat(filepath, filename);
	//printf("path is = %s\n",filepath);
}
 
int 
tic_crow::AppCrow::DeleteFile(const char* path)
{
    DIR *dir;
    struct dirent *dirinfo;
    struct stat statbuf;
    char filepath[256] = {0};
    lstat(path, &statbuf);
    
    if (S_ISREG(statbuf.st_mode))//判断是否是常规文件
    {
        
        remove(path);
    }
    else if (S_ISDIR(statbuf.st_mode))//判断是否是目录
    {
        if ((dir = opendir(path)) == NULL){
            ROS_INFO("open %s fail",path);
            return 1;
        }    
        while ((dirinfo = readdir(dir)) != NULL)
        {
            Getfilepath(path, dirinfo->d_name, filepath);
            if (strcmp(dirinfo->d_name, ".") == 0 || strcmp(dirinfo->d_name, "..") == 0)//判断是否是特殊目录
            continue;
            DeleteFile(filepath);
            rmdir(filepath);
        }
        closedir(dir);
	//delete folder
	rmdir(path);
    }
    return 0;
}

tic_crow::AppCrow::AppCrow(){
	vel_pub = n.advertise<geometry_msgs::Twist>("/keyboard_cmd_vel", 1000);
	initialpose_pub = n.advertise<geometry_msgs::PoseWithCovarianceStamped>("/initialpose",1000);	
	finish_pathrecord = n.advertise<std_msgs::String>("/PathRecordOrPlay", 128);
	boardcmdPub = n.advertise<diagnostic_msgs::DiagnosticStatus>("/WashManager", 1);
	pub_task = n.advertise<std_msgs::Bool>("/PauseTask",1000);
	stop_task = n.advertise<std_msgs::Bool>("/StopTask",1000);

	Rotate_client = n.serviceClient<movemotor_presisecontrol::StartRotate> ("/device/start_rotate_avoid");
	checkRotate_client = n.serviceClient<movemotor_presisecontrol::CheckRotate> ("/device/check_rotate_avoid");
    parameters_client = n.serviceClient<dynamic_reconfigure::Reconfigure>("/pathplay/set_parameters");

    bcmd = {0};

    msg.name = "WashManager";
    msg.message = "WashManager";
    msg.hardware_id = "WashManager";

    kv.key = "brushLevel";
    kv.value = std::to_string(bcmd.brushLevel);
    msg.values.push_back(kv);
    kv.key = "brushLift";
    kv.value = std::to_string(bcmd.brushLift);
    msg.values.push_back(kv);
    kv.key = "waterSpray";
    kv.value = std::to_string(bcmd.waterSpray);
    msg.values.push_back(kv);
    kv.key = "wiperLift";
    kv.value = std::to_string(bcmd.wiperLift);
    msg.values.push_back(kv);
    kv.key = "vacuum";
    kv.value = std::to_string(bcmd.vacuum);
    msg.values.push_back(kv);
    kv.key = "shakeLevel";
    kv.value = std::to_string(bcmd.shakeLevel);
    msg.values.push_back(kv);

    temp.name = "brushLevel";   
    temp.value = bcmd.brushLevel; 
    srv.request.config.ints.push_back(temp);
    
    temp.name = "brushLift";    
    temp.value = bcmd.brushLift; 
    srv.request.config.ints.push_back(temp);
    
    temp.name = "waterSpray";   
    temp.value = bcmd.waterSpray; 
    srv.request.config.ints.push_back(temp);
    
    temp.name = "vacuum";       
    temp.value = bcmd.vacuum; 
    srv.request.config.ints.push_back(temp);
    
    temp.name = "wiperLift";    
    temp.value = bcmd.wiperLift; 
    srv.request.config.ints.push_back(temp);
    
    temp.name = "vibration";    
    temp.value = bcmd.wiperLift; 
    srv.request.config.ints.push_back(temp);
}

tic_crow::AppCrow::~AppCrow(){}
